#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     MiddleMtr,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     DCArmLift,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMtr,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMtr,      tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    Claw,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#define SERVO_START_POS 0
#define SERVOS_MAX_VALUE 250
#define SERVOS_MIN_VALUE 5
#define SERVOS_WAIT_TIME_10MSEC 1

int wristPos = 0;
int clawPos = 0;
int clawOriginalPos = 0;

int armPower = 20;

int deboostPower = 128;

void initializeRobot()
{
	motor[LeftMtr] = 0;
	motor[RightMtr] = 0;
	motor[MiddleMtr] = 0;
	motor[DCArmLift] = -1*armPower;  // raise the arm from rest position...
	wait10Msec(5);   // 50 milliseconds - wait time to lift the arm when teleop begins...
	motor[DCArmLift] = armPower/4;  // Lock it at approx. 135 degrees from ground...

	// Place code here to sinitialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	wristPos = ServoValue[WristLift];
	clawPos = ServoValue[Claw];
	clawOriginalPos = clawPos;

	return;
}

void displayDebugData(int value)
{
	nxtDisplayClearTextLine(2);
	nxtDisplayTextLine(2, "%d", value);
	while(nNxtButtonPressed != 3)
		NULL;
	nxtDisplayClearTextLine(2);
}

void turnL (int power, int duration)
{
  motor[LeftMtr]= -1*power;
  motor[RightMtr]= power;
  motor[MiddleMtr] = 0;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMtr]= power;
  motor[RightMtr]= power;
  motor[MiddleMtr] = 0;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMtr]= power;
  motor[RightMtr]= -1*power;
  motor[MiddleMtr] = 0;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMtr]= -1*power;
  motor[RightMtr]= -1*power;
  motor[MiddleMtr] = 0;
  wait1Msec(duration);
}

task main()
{
	int threshold = 20;             /* Int 'threshold' will allow us to ignore low       */
	initializeRobot();

	waitForStart();     // wait for start of tele-op phase

	while (true)
	{
		getJoystickSettings(joystick);

		// Move the right motor - both forward and backward.  Backward motion happens when Y1 value is less than 0.  Value that comes from
		//    JoyStick control is going to be transferred to the motor as required power.
		if(joystick.joy1_TopHat == 0)
		{
			Forward(50,50);
		}

		else if(joystick.joy1_TopHat == 2)
		{
			turnR(30,50);
		}

		else if(joystick.joy1_TopHat == 4)
		{
			Backward(30,50);
		}

		else if(joystick.joy1_TopHat == 6)
		{
			turnL(50,50);
		}

		else
		{
			motor[LeftMtr] = 0;
			motor[RightMtr] = 0;
			motor[MiddleMtr] = 0;
		}

		// Move the middle motor - When button 8 is presessed, move in one direction and when button 7 is pressed move in opp. direction
		//    If neither of the buttons is pressed, stop move the middle motor.
		//    This motor slides the robot in right or left sides.
		if(joy1Btn(8))
		{
			motor[MiddleMtr] = 50;
		}
		else if(joy1Btn(7))
		{
			motor[MiddleMtr] = -50;
		}
		else
		{
			motor[MiddleMtr] = 0;
		}

		// This is where the ARM program begins

		if (joy2Btn(8))
		{
			armPower = 40;		// If the boost button is pressed, have the power at 40...
		}
		else
		{
			armPower = 20;		// else -- if boost button is NOT pressed, have the power at 20....
		}
		if(abs(joystick.joy2_y1) > threshold)
		{
			if (joystick.joy2_y1 > 0)
			{
				motor[DCArmLift] = armPower;
			}
			else if (joystick.joy2_y1 < 0)
			{
				motor[DCArmLift] = -1*armPower;
			}
		}
		else
		{
			motor[DCArmLift] = armPower/4;  // to stop we need 25% of original power.
		}

		// This where WristLift begins///////////////
		if(abs(joystick_joy2_y2) > threshold)
		{
			if (joystick.joy2_y2 > 0)
			{
				if (wristPos < SERVOS_MAX_VALUE)
				{
					wristPos = wristPos+5;
				}
			}
			else if (joystick.joy2_y2 < 0)
			{
				if (wristPos > SERVOS_MIN_VALUE)
				{
					wristPos = wristPos-5;
				}
			}
			servoTarget[WristLift] = wristPos;
			wait10Msec(SERVOS_WAIT_TIME_10MSEC);
		}

		//This is where Claw begins/////////////////

		if(joy2Btn(1))
		{
			if (clawPos < SERVOS_MAX_VALUE)
			{
				clawPos = clawPos+5;
			}
		}
		if (joy2Btn(3))
		{
			if (clawPos > SERVOS_MIN_VALUE)
			{
				clawPos = clawPos-5;
			}
		}
		servoTarget[Claw] = clawPos;
		wait10Msec(SERVOS_WAIT_TIME_10MSEC);
	}
}
